/**
 * @file    robot_base.h
 * @brief   Robot basic drive to communicate with controller
 * @author  Mr.Wu
 * @date    2025-05-28
 *
 * Copyright 2025 Mr.Wu
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#pragma once

#include <geometry_msgs/msg/transform_stamped.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <tf2/utils.h>
#include <tf2_ros/transform_broadcaster.h>

#include <serial/serial.h>

#define ROBOT_WHEEL_TRACK (0.11) // millimeter

// 上下位机通信协议数据格式
typedef struct {
  uint8_t header;
  uint8_t id;
  uint8_t length;
  uint8_t data[6];
  uint8_t check;
  uint8_t tail;
} DataFrame;

// IMU数据结构体
typedef struct {
  float acceleration_x;
  float acceleration_y;
  float acceleration_z;
  float angular_x;
  float angular_y;
  float angular_z;
  float roll;
  float pitch;
  float yaw;
} DataImu;

typedef struct {
  float x{};     // 机器人在x轴的里程累计位置
  float y{};     // 机器人在y轴的里程累计位置
  float theta{}; // 机器人在z轴的姿态角
} DataOdom;

// 机器人状态结构体
typedef struct {
  float battery_voltage;
  bool buzzer_on;
  bool led_on;
} RobotStatus;

// 帧ID枚举
enum {
  FRAME_ID_MOTION = 0x01, // 速度控制
  FRAME_ID_VELOCITY = 0x02, // 下位机向上位机的速度反馈,用于里程计
  FRAME_ID_ACCELERATION = 0x03, // IMU加速度
  FRAME_ID_ANGULAR = 0x04,      // IMU角速度
  FRAME_ID_EULER = 0x05,        // 姿态欧拉加
  FRAME_ID_SENSOR = 0x06,       // 传感器数据
  FRAME_ID_HMI = 0x07,
};

class RobotBase : public rclcpp::Node {
public:
  RobotBase(std::string name);
  ~RobotBase();

private:
  /**
   * @brief 读取串口数据
   */
  void _readData();
  /**
   * @brief 校验数据帧的合法性
   * @param frame 数据帧
   * @return true 合法 false 非法
   */
  bool _checkDataFrame(DataFrame &frame);
  /**
   * @brief 转换角度到[-PI,PI]范围
   * @param angle 角度
   */
  void _transAngleInPI(float &angle);
  /**
   * @brief 运动学正解
   * @param left_speed 左轮速度,m/s
   * @param right_speed 右轮速度,m/s
   * @param linear_speed 线速度,m/s
   * @param angle_speed 角速度,rad/s
   */
  void _kinematicForward(float left_speed, float right_speed,
                         float &linear_speed, float &angle_speed);
  /**
   * @brief 里程计和TF发布
   * @param linear_speed 线速度,m/s
   * @param angle_speed 角速度,rad/s
   */
  void _odomAndTFPublish(float linear_speed, float angle_speed);
  /**
   * @brief 发布IMU数据
   */
  void _imuPublish();
  /**
   * @brief IMU校准
   */
  bool _imuCalibration();
  /**
   * @brief 蜂鸣器控制
   * @param on true 打开 false 关闭
   */
  bool _buzzerControl(bool on);
  /**
   * @brief 速度控制
   * @param left_speed 左轮速度,m/s
   * @param right_speed 右轮速度,m/s
   * @return true 成功 false 失败
   */
  bool _velocityControl(float left_speed, float right_speed);
  /**
   * @brief 处理数据帧
   * @param frame 数据帧
   * @return void
   */
  void _processDataFrame(DataFrame &frame);
  /**
   * @brief 处理速度数据
   * @param frame 数据帧
   */
  void _processVelocityData(DataFrame &frame);
  /**
   * @brief 处理角速度数据
   * @param frame 数据帧
   */
  void _processAngularVelData(DataFrame &frame);
  /**
   * @brief 处理加速度数据
   * @param frame 数据帧
   */
  void _processAccelerationData(DataFrame &frame);
  /**
   * @brief 处理欧拉角数据
   * @param frame 数据帧
   */
  void _processEulerData(DataFrame &frame);
  /**
   * @brief 处理传感器数据
   * @param frame 数据帧
   */
  void _processSensorData(DataFrame &frame);

  /**
   * @brief 速度控制回调函数
   */
  void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg);
  /**
   * @brief 定时器回调函数
   */
  void timer_callback();

private:
  // parameter
  std::string port_name_{"ttyS1"};
  uint32_t baudrate_{115200};
  float correct_factor_vx_{
      1.0}; // 速度校准系数=实际运行距离/odom反馈距离，多次测量求取平均值
  float correct_factor_wz_{
      1.0}; // 角速度修正系数=实际旋转角度/odom累计角度，多次测量求取平均值
  bool pub_odom_tf_{false}; // 是否发布odom的tf
  bool use_imu_{false};     // 是否使用IMU数据
  bool auto_stop_on_{
      false}; // 自动停止标志位,如果没有这个标识，又没有导航控制，机器人一旦运行会发送碰撞

  // variable
  rclcpp::Time current_time_{};
  int auto_stop_count_{}; // 自动停止计数器

  // object
  // 默认使用ttyS3串口，115200的波特率
  // serial::Serial serial_{"/dev/ttyS3", 115200};
  // 不可直接传递构造，会直接尝试打开串口
  serial::Serial serial_;
  DataOdom odom_{};    // 里程计数据
  DataImu imu_data_{}; // IMU数据
  rclcpp::TimerBase::SharedPtr timer_{};

  // topic client
  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_{};
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_{};
  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_{};

  // topic service
  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr
      cmd_vel_sub_{}; // 速度控制订阅
};